#ifndef RSIM_QUATERNION_H
#define RSIM_QUATERNION_H

/* -------------------------------------------------------------------------- *
 * File: Quaternion.h                                                         *
 * Authors: Kishor Bhalerao                                                   *
 * Email : kishor8dm@gmail.com                                                *
 * Contributors:                                                              *
 * Email:                                                                     *
 * copyright (c) 2010 Authors.                                                *
 *                                                                            *
 * This program is free software: you can redistribute it and/or modify it    *
 * under the terms of the GNU General Public License as published by the Free *
 * Software Foundation, either version 3 of the License, or any later version.*
 *                                                                            *
 * This program is distributed in the hope that it will be useful, but        *
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY *
 * or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License   *
 * for more details. (http://www.gnu.org/licenses/)                           *
 * -------------------------------------------------------------------------- */

#include "Vect4.h"
#include "LinAlgDecl.h"

namespace RSIM{

class Rotation;
        
/** 	Quaternion is a 4x1 vector [e0 e1 e2 e3] with e0 being the scalar.
        Thus if rotation about quaternion vector v is theta, then the 
        quaternions are given by 
        e0 = cos(theta/2)
        [e1 e2 e3] = v*sin(theta/2)
        
        Useful results (matlab syntax). For a body B at an orientation given by 
        euler parameters e = [e0 e1 e2 e3] with respect to a newtoanian/parent 
        frame N, we have
        
        R_NB = EG'; where
                
        E = [-e1  e0 -e3  e2;
             -e2  e3  e0 -e1;
             -e3 -e2  e1  e0];
        
        G = [-e1  e0  e3 -e2;
             -e2 -e3  e0  e1;
             -e3  e2 -e1  e0];
             
        and G*G' = E*E' = I (3 x 3 Identity Matrix);        
        
        {w_NB}_B = Angular velocity of B with respect to N *EXPRESSED IN B* = 2*G*dot(e)
        {w_NB}_N = Angular velocity of B with respect to N *EXPRESSED IN N* = 2*E*dot(e)
        
        \cite{Shivarama2004}, \cite{Farid}
*/
class Quaternion:public Vect4_<double>{
        public:
                /** Default constructor sets the quaternion to zero rotation */
                Quaternion();
                
                /** set quaternion from vect4 */
                explicit Quaternion(const Vect4& q);
                
                /** set quaternion from double */
                Quaternion(const double& e0, const double& e1, 
                        const double& e2, const double& e3);
                                        
                /** set quaternion from rotation matrix. No checks to ensure 
                that Rot is a valid Rotation matrix. */
                explicit Quaternion(const Rotation& Rot);
                
                /** If w is the relative angular velocity of a body with respect to its parent, q
                the relative quaternion, then the time derivatives of relative quaternions of this 
                body are given by edot. Note that quaternion here is [e0 e1 e2 e3] where e0 is the scalar.                 
                */
                static void calcQDotFromAngularVelocity(const double *w, 
                                                        const double *q,
                                                        double *EDot);
                
                static void calcAngularVelocityFromEDot(const double *e,
                                                        const double *edot,
                                                        double *w);
                
                /** Same as static void calcQDotFromAngularVelocity(const double *, const double *, double *)*/
                void calcQDotFromAngularVelocity(const double *w,
                                                 double *EDot)const;
                                
                static void calcQuaternionFromEulerAngles(      const double& ang1,
                                                                const double& ang2,
                                                                const double& ang3,
                                                                const char *ptr,
                                                                double *Quat);
                
                /** Normalize the quaternion. This is needed to get rid
                of the drift introduced in the simulation on numerical
                integration. */
                void normalize();
};

// @article{Shivarama2004,
//   title={{Hamilton’s equations with Euler parameters for rigid body dynamics modeling}},
//   author={Shivarama, R. and Fahrenthold, E.P.},
//   journal={Journal of dynamic systems, measurement, and control},
//   volume={126},
//   pages={124},
//   year={2004}
// }

// @book{Farid,
// title={{Fundamentals of multibody dynamics: theory and applications}},
// author={Amirouche, F.M.L.},
// year={2006},
// publisher={Birkhauser}
// }


} // namespace RSIM

#endif